/*
*Copyright (c) 2025 Huawei Device Co., Ltd. All rights reserved.
*Licensed under the Apache License, Version 2.0 (the "License");
*you may not use this file except in compliance with the License.
*You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

*Unless required by applicable law or agreed to in writing, software
*distributed under the License is distributed on an "AS IS" BASIS,
*WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*See the License for the specific language governing permissions and
*limitations under the License.
*/

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <opencv2/opencv.hpp>
#include <filesystem>
#include <vector>
#include <chrono>

using namespace std::chrono_literals;
namespace fs = std::filesystem;

namespace apple_detect_demo {
class ApplePublisher : public rclcpp::Node {
    // 参数
    std::string image_dir_;
    double publish_rate_;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::vector<std::string> image_files_;
    size_t current_index_;
    static constexpr size_t kQueueSize = 10;
    static constexpr double kMillisecondsPerSecond = 1000.0;
    static constexpr int kImageChannels = 3; // 图像通道数

public:
    ApplePublisher() : Node("apple_publisher"), current_index_(0), publish_rate_(1.0)
    {
        // 设置图像目录
        image_dir_ = "apple_images";

        // 初始化发布者
        publisher_ = this->create_publisher<sensor_msgs::msg::Image>("raw_images", kQueueSize);

        // 加载图像目录
        if (!fs::exists(image_dir_)) {
            RCLCPP_ERROR(this->get_logger(), "Image directory not found: %s", image_dir_.c_str());
            rclcpp::shutdown();
            return;
        }

        // 获取图像文件列表
        for (const auto &entry : fs::directory_iterator(image_dir_)) {
            std::string ext = entry.path().extension().string();
            if (ext == ".jpg" || ext == ".png") {
                image_files_.push_back(entry.path().string());
                RCLCPP_INFO(this->get_logger(), "Found image: %s", entry.path().filename().string().c_str());
            }
        }

        if (image_files_.empty()) {
            RCLCPP_ERROR(this->get_logger(), "No images found in directory");
            rclcpp::shutdown();
            return;
        }
        // 设置定时器
        timer_ =
            this->create_wall_timer(std::chrono::milliseconds(static_cast<int>(kMillisecondsPerSecond / publish_rate_)),
            std::bind(&ApplePublisher::publish_image, this));

        RCLCPP_INFO(this->get_logger(), "Publishing %zu images at %.1f Hz", image_files_.size(), publish_rate_);
    }

private:
    void publish_image()
    {
        cv::Mat image = cv::imread(image_files_[current_index_]);
        if (image.empty()) {
            RCLCPP_ERROR(this->get_logger(), "Failed to load image: %s", image_files_[current_index_].c_str());
            return;
        }

        // 转换为ROS消息
        auto msg = std::make_shared<sensor_msgs::msg::Image>();
        msg->header.stamp = this->now();
        msg->header.frame_id = "camera";
        msg->height = image.rows;
        msg->width = image.cols;
        msg->encoding = "bgr8";
        msg->step = image.cols * kImageChannels;
        msg->data.assign(image.datastart, image.dataend);

        publisher_->publish(*msg);
        RCLCPP_DEBUG(this->get_logger(), "Published image %zu/%zu", current_index_ + 1, image_files_.size());

        current_index_ = (current_index_ + 1) % image_files_.size();
    }
};
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<apple_detect_demo::ApplePublisher>());
    rclcpp::shutdown();
    return 0;
}